#include "rc.h"
#include "hardware.h"

int16_t rc_channel_to_range(rc_channel* rc, int16_t value)
{
	int16_t trim_high = rc->rc_trim + rc->rc_dz;
	int16_t trim_low = rc->rc_trim - rc->rc_dz;
	int16_t reverse_mul = rc->rc_reverse?-1:1;
	
	if(value>trim_high)
	{
		rc->target = reverse_mul * ((int32_t)rc->target_range * (int32_t)(value - trim_high)) / (int32_t)(rc->rc_max  - trim_high);
		if(rc->target > rc->target_range)
			rc->target = rc->target_range;
		return rc->target;
	}
	else if(value<trim_low)
	{
		rc->target = reverse_mul * ((int32_t)rc->target_range * (int32_t)(value - trim_low)) / (int32_t)(trim_low - rc->rc_min);
		if(rc->target < -rc->target_range)
			rc->target = -rc->target_range;
		return rc->target;
	}
	else
	{
		rc->target = 0;
		return 0;
	}
}

void rc_channel_set_reverse(rc_channel* rc, uint8_t re)
{
	rc->rc_reverse = re;
}

void rc_channel_set_range(rc_channel* rc, int16_t range)
{
	rc->target_range = range;
}

void rc_channel_set(rc_channel* rc, int16_t max, int16_t min, int16_t dz)
{
	rc->rc_max = max;
	rc->rc_min = min;
	rc->rc_dz = dz;
	rc->rc_trim = min+((max-min)/2);
}
